#include <ESP8266WiFi.h>
#include <ArduinoOTA.h>
#include <WiFiManager.h>
#include <PubSubClient.h>
#include "HC_SR04.h"
#include "sport.h"
#include <Ticker.h>

const char* mqttServer = "1.14.32.185";
const char* topic1 = "4Gcar/move/FB";
const char* topic2 = "4Gcar/move/LR";
const char* topic3 = "4Gcar/move/dierction_offset";
extern int cm;
extern int midservo;

WiFiManager wifiManager;
WiFiClient wifiClient;
PubSubClient mqttClient(wifiClient);
Ticker ticker;

String order;
int flag = 0;

void setup() {
  Serial.begin(115200);

  WiFi.mode(WIFI_STA);

  wifiManager.autoConnect("4G Car Pro");

  mqttClient.setServer(mqttServer, 1883);
  mqttClient.setCallback(receiveCallback);
  connectMQTTserver();

  ArduinoOTA.setHostname("4G car");
  ArduinoOTA.setPassword("88888888");
  ArduinoOTA.begin();

  Turn_Init();
  Motor_Init();

  //ticker.attach(0.50, Stop_move);
  //HC_SR04_Init();
}

void loop() {
  if (mqttClient.connected()) {  // 如果开发板成功连接服务器
    mqttClient.loop();           // 处理信息以及心跳
  } else {                       // 如果开发板未能成功连接服务器
    connectMQTTserver();         // 则尝试连接服务器
  }
  ArduinoOTA.handle();
}

// 连接MQTT服务器并订阅信息
void connectMQTTserver() {
  String clientId = "Car";

  // 连接MQTT服务器
  if (mqttClient.connect(clientId.c_str())) {
    subscribeTopic();  // 订阅指定主题
  } else {
    Serial.print("MQTT Server Connect Failed. Client State:");
    Serial.println(mqttClient.state());
    delay(5000);
  }
}

// 订阅指定主题
void subscribeTopic() {
  String topicString = "4Gcar/move/#";
  char subTopic[topicString.length() + 1];
  strcpy(subTopic, topicString.c_str());


  // 通过串口监视器输出是否成功订阅主题以及订阅的主题名称
  if (mqttClient.subscribe(subTopic)) {
    Serial.println("Subscrib Topic success!");
  } else {
    Serial.print("Subscribe Fail...");
  }
}

// 收到信息后的回调函数
void receiveCallback(char* topic, byte* payload, unsigned int length) {
  // Serial.print("Message Received [");
  // Serial.print(topic);
  // Serial.println("]: ");
  // Serial.println((char)payload[0]);
  if (strcmp(topic, topic1) == 0) {
    Switch_Order1((char)payload[0]);
  }
  if (strcmp(topic, topic2) == 0) {
    Switch_Order2((char)payload[0]);
  }
  if (strcmp(topic, topic3) == 0) {
    Switch_Order3((char)payload[0]);
  }
}

//驱动电机+速度切换
void Switch_Order1(int order) {
  switch (order) {
    case '0':
      {
        digitalWrite(12, 0);
        digitalWrite(13, 0);
        break;
      }
    case '1':
      {
        //Serial.println("前进");
        GO_BACK(0);
        break;
      }
    case '2':
      {
        //Serial.println("后退");
        GO_BACK(1);
        break;
      }
    case '3':
      {
        SPEED(1);  //加速
        break;
      }
    case '4':
      {
        SPEED(0);  //减速
        break;
      }
  }
}
void Switch_Order2(int order) {
  switch (order) {
    case '0':
      {
        servo_mid();
        break;
      }
    case '1':
      {
        //Serial.println("左转");
        turn_to(1);
        break;
      }
    case '2':
      {
        //Serial.println("右转");
        turn_to(0);
        break;
      }
  }
}
void Switch_Order3(int order) {
  switch (order) {
    case '0':
      {
        midservo += 1;  //左
        if (midservo >= 140)
          midservo = 140;
        break;
      }
    case '1':
      {
        midservo -= 1;  //右
        if (midservo <= 40)
          midservo = 40;
        break;
      }
  }
}
void Stop_move() {
  digitalWrite(12, 0);
  digitalWrite(13, 0);
  servo_mid();
  //Serial.println("停止");
}